#!/usr/bin/env python

import rospy
import time
import random
from gazebo_msgs.msg import ModelState, ModelStates
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Twist

class Combination():
    def __init__(self):
        self.pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1)
        self.moving()

    def moving(self):
        while not rospy.is_shutdown():
            model = rospy.wait_for_message('gazebo/model_states', ModelStates)
            for i in range(len(model.name)):
                if model.name[i] == 'turtlebot3_burger':
                    turtlebot_pos= ModelState()
                    turtlebot_pos.model_name=model.name[i]
                    turtlebot_pos.pose = model.pose[i]
                    if turtlebot_pos.pose.position.x < 0.03 and turtlebot_pos.pose.position.y < 0.03:
                        print("ja<0.01")
                        for j in range(len(model.name)):
                            if model.name[j] == 'wall_9':
                                print ("wall_9")
                                wall_pos= ModelState()
                                wall_pos.model_name=model.name[j]
                                wall_pos.pose = model.pose[j]
                                a=random.randint(1, 2)
                                print(a)
                                if a==1:
                                    wall_pos.pose.position.x = 2
                                elif a==2:
                                    wall_pos.pose.position.x = 0
                                print(wall_pos.pose.position.x)
                    self.pub_model.publish(wall_pos)
                    rospy.sleep(1)

def main():
    rospy.init_node('simulation_5_obstacle_1')
    try:
        combination = Combination()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()
